#include <stdlib.h>

#include "../memmap.h"
#include "../host/host.h"
#include "../log.h"

#include "color.h"
#include "display.h"
#include "ge.h"
#include "gpu.h"
#include "light.h"
#include "texture.h"
#include "vector.h"
#include "vertexdec.h"

#include "pipeline.h"

float worldMatrix[12] =
{
    1, 0, 0,
    0, 1, 0,
    0, 0, 1,
    0, 0, 0,
};

float viewMatrix[12] =
{
    1, 0, 0,
    0, 1, 0,
    0, 0, 1,
    0, 0, 0
};

float projMatrix[16] =
{
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 1
};

float tgenMatrix[12] =
{
    1, 0, 0,
    0, 1, 0,
    0, 0, 1,
    0, 0, 0
};

float boneMatrix[8 * 12];

u8 oldThrough = 0;

u32 factorial(u32 v)
{
    u32 i;
    u32 res = 1;
    for (i = 2; i < v + 1; i++)
        res *= i;
    return res;
}

void bernstein(float n, float res[4])
{
    float n2 = n * n;
    float m = 1 - n;
    float m2 = m * m;
    res[0] = m2 * m;
    res[1] = 3 * m2 * n;
    res[2] = 3 * m * n2;
    res[3] = n2 * n;
}

void calcBezier(Vertex *in, Vertex *out, u32 ucount, u32 vcount, u8 noTc)
{
    u32 s, t;
    u32 upcount = ucount / 3; /* Each curve is composed by 4 points, with the last one being the first one of the next curve */
    u32 vpcount = vcount / 3;
    memset(out, 0, sizeof(Vertex) * (gstate.psubt + 1) * (gstate.psubs + 1));
    for (t = 0; t <= gstate.psubt; t++)
    {
        /* The global fraction of the final curve: first curve goes from 0 to 1, second from 1 to 2, etc. with intermediates depending on psubs / psubt */
        float vglobal = (float)(t * vpcount) / (float)gstate.psubt;

        u32 vpatch = vglobal; /* The curve number (starting with 0) */
        float v = vglobal - vpatch; /* The 'local' fraction, from 0 to 1 */
        float vbern[4];

        if (t == gstate.psubt) {
            vpatch--;
            v = 1.0f;
        }

        bernstein(v, vbern);
        for (s = 0; s <= gstate.psubs; s++)
        {
            float uglobal = (float)(s * upcount) / (float)gstate.psubs;

            u32 upatch = uglobal;
            float u = uglobal - (float)upatch;
            u32 i, j;
            float ubern[4];
            Vertex *curOut = &out[s * (gstate.psubt + 1) + t];

            if (s == gstate.psubs) {
                upatch--;
                u = 1.0f;
            }

            bernstein(u, ubern);
            for (i = 0; i < 4; i++)
            {
                for (j = 0; j < 4; j++)
                {
                    u8 k;
                    float coef = ubern[i] * vbern[j];
                    Vertex *curIn = &in[(3 * upatch + i) * vcount + 3 * vpatch + j];
                    for (k = 0; k < 3; k++)
                        curOut->v[k] += coef * curIn->v[k];
                    for (k = 0; k < 4; k++)
                        curOut->c[k] += coef * curIn->c[k];
                    if (!noTc)
                        for (k = 0; k < 2; k++)
                            curOut->uv[k] += coef * curIn->uv[k];
                }
            }
            if (noTc) {
                curOut->uv[0] = uglobal;
                curOut->uv[1] = vglobal;
            }
        }
    }
}

void initProjection(u8 throughmode)
{
    host_resetMatrices();

    if (gstate.projChanged || oldThrough != throughmode)
    {
        if (throughmode)
        {
            /* 2D through mode */
            host_setCapability(RE_CAPABILITY_CULL_FACE, 0);
            host_setViewport(0, 0, 480, 272);
            host_setOrthoProj();
        }
        else
        {
            /* Full 3D pipeline used */
            host_setCapability(RE_CAPABILITY_CULL_FACE, gstate.cullFaceEnabled);
            host_setViewport(gstate.xpos - gstate.offsetx - gstate.xscale,
                             gstate.ypos - gstate.offsety - gstate.yscale,
                             gstate.xscale * 2,
                             gstate.yscale * 2);
            host_loadProjMatrix(projMatrix);
            host_setDepthRange(gstate.zpos - gstate.zscale, gstate.zpos + gstate.zscale);
        }
    }

    oldThrough = throughmode;
}

void initTexturing()
{
    if (gstate.textureChanged)
    {
        if (gstate.textureMapEnable && !(gstate.clearmode & 1)) {
            host_setCapability(RE_CAPABILITY_TEXTURE_2D, 1);
            setTexture();
        }
        else
            host_setCapability(RE_CAPABILITY_TEXTURE_2D, 0);
    }
}

void readVertAndTrans(VertexDecoder *vd, PtrVertex *vert, u32 vertsAddr, u32 indsAddr, u32 cur)
{
    float weights[8], normals[3];
    vert->normal = normals;
    vert->wt = weights;
    vertex_decode(vd, vert, vertsAddr, indsAddr, cur);

    if (!vd->throughmode)
    {
        float out[3], norm[3];

        vert->uv[0] = vert->uv[0] * gstate.uScale + gstate.uOff;
        vert->uv[1] = vert->uv[1] * gstate.vScale + gstate.vOff;

        /* We do software T&L for now */
        if (vd->weighttype == GE_VTYPE_WEIGHT_NONE) {
            Vec3ByMatrix43(out, vert->v, worldMatrix);
            Norm3ByMatrix43(norm, vert->normal, worldMatrix);
        }
        else
        {
            Vec3 psum, nsum;
            u32 i;
            vec_initfromxyz(&psum, 0, 0, 0);
            vec_initfromxyz(&nsum, 0, 0, 0);
            for (i = 0; i < vd->nweights; i++)
            {
                Vec3 tpos, tnorm;
                Vec3ByMatrix43(out, vert->v, boneMatrix + i * 12);
                Norm3ByMatrix43(norm, vert->normal, boneMatrix + i * 12);
                vec_initfromarr(&tpos, out);
                vec_initfromarr(&tnorm, norm);
                psum = vec_add(psum, vec_multiplybyflt(tpos,  vert->wt[i]));
                nsum = vec_add(nsum, vec_multiplybyflt(tnorm, vert->wt[i]));
            }
            vec_normalize(&nsum);
            vec_write(&psum, out);
            vec_write(&nsum, norm);
        }

        /* Perform lighting here */
        if (gstate.lightingEnable)
        {
            Vec3 outv, normv;
            vec_initfromxyz(&outv, out[0], out[1], out[2]);
            vec_initfromxyz(&normv, norm[0], norm[1], norm[2]);
            light(vert->c, vert->uv, vert->c, outv, normv);
        }
        Vec3ByMatrix43(vert->v, out, viewMatrix);
    }
}

void primPipeline(u32 vertsAddr, u32 indsAddr, GEPrimitiveType prim, u32 count)
{
    u32 i;
    float *colorArray, *texCoordArray, *vertexArray;
    VertexDecoder vd;

    colorArray    = malloc(sizeof(float) * count * 4);
    texCoordArray = malloc(sizeof(float) * count * 2);
    vertexArray   = malloc(sizeof(float) * count * 3);

    vertexdec_setVertexType(&vd, gstate.vertType);
    initProjection(vd.throughmode);
    initTexturing();

    for (i = 0; i < count; i++)
    {
        PtrVertex vert;

        vert.c =  &colorArray   [i * 4];
        vert.uv = &texCoordArray[i * 2];
        vert.v =  &vertexArray  [i * 3];

        readVertAndTrans(&vd, &vert, vertsAddr, indsAddr, i);
    }
    
    host_drawPrimitive(vertexArray, texCoordArray, colorArray, prim, count);

    display_sceneChanged();

    free(colorArray);
    free(texCoordArray);
    free(vertexArray);
}

void spritePipeline(u32 vertsAddr, u32 indsAddr, u32 count)
{
    u32 i, j;
    float *colorArray, *texCoordArray, *vertexArray;
    float c[4], uv[2][2], v[2][3];
    u32 cur = 0;
    VertexDecoder vd;

    colorArray    = malloc(sizeof(float) * 2 * count * 4);
    texCoordArray = malloc(sizeof(float) * 2 * count * 2);
    vertexArray   = malloc(sizeof(float) * 2 * count * 3);

    vertexdec_setVertexType(&vd, gstate.vertType);
    initProjection(vd.throughmode);
    initTexturing();

    for (i = 0; i < count; i++)
    {
        PtrVertex vert;

        vert.c =  c;
        vert.uv = (float*)&uv[i & 1];
        vert.v =  (float*)& v[i & 1];

        readVertAndTrans(&vd, &vert, vertsAddr, indsAddr, i);

        if ((i & 1) == 1)
        {
            for (j = 0; j < 4; j++)
                memcpy(&colorArray[(cur + j) * 4], c, sizeof(float) * 4);

            /* Create 4 rectangle vertice & texture coords, based on the coords of the upper-left and lower-right corners */
            texCoordArray[(cur + 0) * 2 + 0] = uv[1][0];
            texCoordArray[(cur + 0) * 2 + 1] = uv[1][1];
            texCoordArray[(cur + 1) * 2 + 0] = uv[0][0];
            texCoordArray[(cur + 1) * 2 + 1] = uv[1][1];
            texCoordArray[(cur + 2) * 2 + 0] = uv[0][0];
            texCoordArray[(cur + 2) * 2 + 1] = uv[0][1];
            texCoordArray[(cur + 3) * 2 + 0] = uv[1][0];
            texCoordArray[(cur + 3) * 2 + 1] = uv[0][1];

            vertexArray[(cur + 0) * 3 + 0] = v[1][0];
            vertexArray[(cur + 0) * 3 + 1] = v[1][1];
            vertexArray[(cur + 0) * 3 + 2] = v[0][2];
            vertexArray[(cur + 1) * 3 + 0] = v[0][0];
            vertexArray[(cur + 1) * 3 + 1] = v[1][1];
            vertexArray[(cur + 1) * 3 + 2] = v[0][2];
            vertexArray[(cur + 2) * 3 + 0] = v[0][0];
            vertexArray[(cur + 2) * 3 + 1] = v[0][1];
            vertexArray[(cur + 2) * 3 + 2] = v[0][2];
            vertexArray[(cur + 3) * 3 + 0] = v[1][0];
            vertexArray[(cur + 3) * 3 + 1] = v[0][1];
            vertexArray[(cur + 3) * 3 + 2] = v[0][2];
            cur += 4;
        }
    }
    
    host_drawPrimitive(vertexArray, texCoordArray, colorArray, GE_PRIM_SPRITES, cur);

    display_sceneChanged();

    free(colorArray);
    free(texCoordArray);
    free(vertexArray);
}

void bezierPipeline(u32 vertsAddr, u32 indsAddr, u32 ucount, u32 vcount)
{
    u32 i;
    float *finalArray, *finalTexArray, *finalColArray;
    Vertex *original, *bezier;
    u32 numVerts = (gstate.psubs + 1) * 2;
    u32 numSubs = (gstate.psubs + 1) * (gstate.psubt + 1);
    VertexDecoder vd;
    
    original = malloc(sizeof(Vertex) * ucount * vcount);
    bezier   = malloc(sizeof(Vertex) * numSubs);

    finalColArray = malloc(sizeof(float) * numVerts * 4);
    finalTexArray = malloc(sizeof(float) * numVerts * 2);
    finalArray    = malloc(sizeof(float) * numVerts * 3);

    vertexdec_setVertexType(&vd, gstate.vertType);
    initProjection(vd.throughmode);
    initTexturing();

    for (i = 0; i < ucount * vcount; i++)
    {
        PtrVertex vert;

        vert.c  = (float*)original[i].c;
        vert.uv = (float*)original[i].uv;
        vert.v  = (float*)original[i].v;

        readVertAndTrans(&vd, &vert, vertsAddr, indsAddr, i);
    }

    calcBezier(original, bezier, ucount, vcount, (vd.tc == 0));

    for (i = 0; i <= gstate.psubt - 1; i++)
    {
        u32 j;
        for (j = 0; j <= gstate.psubs; j++)
        {
            u8 k;
            for (k = 0; k < 2; k++)
            {
                memcpy(&finalArray   [(j * 2 + k) * 3], bezier[j * (gstate.psubt + 1) + i + k].v,  sizeof(float) * 3);
                memcpy(&finalTexArray[(j * 2 + k) * 2], bezier[j * (gstate.psubt + 1) + i + k].uv, sizeof(float) * 2);
                memcpy(&finalColArray[(j * 2 + k) * 4], bezier[j * (gstate.psubt + 1) + i + k].c,  sizeof(float) * 4);
            }
        }
        host_drawPrimitive(finalArray, finalTexArray, finalColArray, GE_PRIM_TRIANGLE_STRIP, (gstate.psubs + 1) * 2);
    }

    display_sceneChanged();

    free(original);
    free(bezier);
    free(finalArray);
    free(finalColArray);
    free(finalTexArray);
}

